﻿using ROSBridgeLib;
using ROSBridgeLib.sensor_msgs;
using ROSBridgeLib.std_msgs;
using ROSBridgeLib.geometry_msgs;
using ROSBridgeLib.nav_msgs;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using SimpleJSON;

public class NavPathSubscriber : BaseSubscriber
{

    // Use this for initialization


    private LineRenderer navPath;
   
    public CurrentGoalSubscriber goalSubscriber;

    public RobotposeSubscriber robotPoseSubscriber;

    public GameObject[] navFinishNotify;

    bool inNav = false;


    void Start()
    {


        topic = "move_base/NavfnROS/plan";

        messageType = "nav_msgs/Path";

        navPath = GetComponent<LineRenderer>();

    }



    public override void OnReset()
    {
        base.OnReset();

        if (navPath)
        {
        navPath.numPositions = 0;
        }
    }

    void Update()
    {


        if (inNav)
        {
            float distance = Vector3.Distance(goalSubscriber.transform.position, robotPoseSubscriber.transform.position);


            if (distance < 0.17f)
            {
            
               
                OnNavFinish();

               
                inNav=false;
            }
        }


    }




    public override ROSBridgeMsg Parse(JSONNode msg)
    {
        return  new PathMsg(msg["msg"]);
    }
   


    public override void OnMessage()
    {
      


        PathMsg msg=(PathMsg)GetTopParsedPackage();

        if (msg==null)
        {
           
            return;

        }

        inNav = true;

    
        navPath.numPositions = msg.poses.Length;

        for (int i = 0; i < msg.poses.Length; i++)
        {

            navPath.SetPosition(i, msg.poses[i].pose.position.ToVector3());

        }


//        Debug.Log(msg.header.GetTimeMsg().ToString());


    }


    public override void OnNavFinish()
    {



        navPath.numPositions = 0;


        foreach (GameObject obj in navFinishNotify)
        {
            
            obj.SendMessage("OnNavFinish",0.1f);

        }

    }
}
